#!/usr/bin/env python

import roslib; roslib.load_manifest('roscolbot')
import rospy

from joy.msg import Joy
from rosqwerk.msg import ServosControl
from rosqwerk.msg import DigitalOutputsControl
from roscolbot.msg import NeckControl
from geometry_msgs.msg import Twist


class JoystickControl():
    """
    Listen for joystick and move colbot accordingly
    """
    def __init__(self):
        rospy.init_node('colbot_joystick_control')              
        rospy.Subscriber('/joy', Joy, self.readJoystickAndMove)       
         
        self.servosControlPublisher = rospy.Publisher("/qwerk/servos_control", ServosControl)
        self.head_pan_servo_value=127
        self.head_tilt_servo_value=127
        
        self.neckControlPublisher = rospy.Publisher("/colbot/neck_control", NeckControl)
        self.neckIsUp=False
        self.neckChangeLastTime=rospy.get_time()
        
        self.digitalOutputsControlPublisher = rospy.Publisher("/qwerk/digitaloutputs_control", DigitalOutputsControl)
        self.laserIsOn=False
        self.laserChangeLastTime=rospy.get_time()
        
        self.moveBaseVelocityPublisher = rospy.Publisher('cmd_vel', Twist)
        self.isMovingBase=False

    
    def readJoystickAndMove(self, joyData):
        moveHead=False
        moveBase=False
        movespeed=0
        turnspeed=0
                
        # 'B'(toggle button on back of device) = 3
        if (joyData.buttons[3]):
            self.isMovingBase=True
            moveBase=True
            # Left=6 ; Right=7
            if (joyData.buttons[7]):  
                turnspeed=-0.3
            elif (joyData.buttons[6]):   
                turnspeed=0.3
            # Up=8 ; 9=Down
            if (joyData.buttons[9]):   
                movespeed=-0.1
            elif (joyData.buttons[8]):   
                movespeed=0.1    
        else:
            if self.isMovingBase:
                self.isMovingBase=False
                moveBase=True
            # Left=6 ; Right=7
            if (joyData.buttons[7]) and (self.head_pan_servo_value>0):  
                self.head_pan_servo_value-=1
                moveHead=True
            elif (joyData.buttons[6]) and (self.head_pan_servo_value<255):   
                self.head_pan_servo_value+=1
                moveHead=True
            # Up=8 ; 9=Down
            if (joyData.buttons[9]) and (self.head_tilt_servo_value>0):   
                self.head_tilt_servo_value-=1
                moveHead=True
            elif (joyData.buttons[8]) and (self.head_tilt_servo_value<255):   
                self.head_tilt_servo_value+=1   
                moveHead=True     
 
        if moveHead:
            msg = ServosControl()
            for i in range(len(msg.servoPositions)):
                msg.servoPositions[i]=-1     
            msg.servoPositions[0]=self.head_pan_servo_value
            msg.servoPositions[1]=self.head_tilt_servo_value
            self.servosControlPublisher.publish(msg)
            
        if moveBase:
            twist = Twist()
            twist.linear.x = movespeed; twist.linear.y = 0; twist.linear.z = 0
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = turnspeed
            self.moveBaseVelocityPublisher.publish(twist)
        
        # 'A'=2
        if ((joyData.buttons[2]) and (rospy.get_time()>self.neckChangeLastTime+2)):  
            self.neckChangeLastTime=rospy.get_time()
            self.neckIsUp=not self.neckIsUp
            msg = NeckControl()            
            msg.neckIsUp=self.neckIsUp
            self.neckControlPublisher.publish(msg)
            
        # '1'=0
        if ((joyData.buttons[0]) and (rospy.get_time()>self.laserChangeLastTime+2)):  
            self.laserChangeLastTime=rospy.get_time()
            self.laserIsOn=not self.laserIsOn
            msg = DigitalOutputsControl()            
            msg.digitalOutMask=[True,True,False,False,False,False,False,False]
            msg.digitalOutStates=[self.laserIsOn,self.laserIsOn,False,False,False,False,False,False]
            self.digitalOutputsControlPublisher.publish(msg)            

if __name__ == '__main__':
    try:
        JoystickControl()
        rospy.spin()
    except rospy.ROSInterruptException:
        twist = Twist()
        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0        
        rospy.Publisher('cmd_vel',Twist).moveBaseVelocityPublisher.publish(twist)
        pass

